﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;

using Microsoft.Xna.Framework.Graphics;
using Drifting.MotionPlanning.Primitives;
using Drifting.MotionPlanning.Systems;
using Drifting.MotionPlanning.PointSets;
using Drifting.Graphics.EffectObjects;
using Drifting.Foundation;

namespace Drifting.MotionPlanning.MultiThreading.Planners
{
    public class SST
    {
        protected class Agent
        {
            // algorithmic fields
            SST master;
            BaseSystem system;
            PointSet Vactive;
            PointSet S;
            BaseSystem.MemoryPad memoryPad;
            Random rand;
            double dv;
            double ds;
            double[] a_sample;
            double[] iCtrl;
            List<State> neighbors;

            // communication fields
            public bool done;
            public Thread workingThread;
            public State x_new;
            public Edge e_new;
            public Representation s_new;

            // measurement
            public int _measurement_n_tasks;

            public Agent(SST master, PointSet v, PointSet S, double dv, double ds)
            {
                rand = new Random();
                this.master = master;
                this.system = master.system;
                Vactive = v;
                this.S = S;
                memoryPad = new BaseSystem.MemoryPad();
                memoryPad.Prepare(system.DOF_State);
                a_sample = new double[system.DOF_Alias];
                x_new = new State(new double[system.DOF_State], new double[system.DOF_Alias]);
                e_new = new Edge();
                e_new.control = new double[system.DOF_Control];
                iCtrl = e_new.control;
                neighbors = null;
                this.dv = dv;
                this.ds = ds;
                _measurement_n_tasks = 0;

                done = false;
                workingThread = new Thread(Execute);
                workingThread.Start();
            }

            public void Execute()
            {
                while (true)
                {
                    if (done == true)
                    {
                        Thread.Sleep(0);
                    }
                    else
                    {
                        //if action succeeds, done is true. Otherwise, restart Action to start a new job.
                        if(Action())
                            done = true;

                        _measurement_n_tasks++;
                    }
                }
            }

            public bool Action()
            {
                //Sample
                system.SampleAlias(rand, ref a_sample);

                //Selection
                State x_nearest = null;
                double dist2_sample = 0;
                Vactive.Neighbor_RangeQuery(a_sample, dv, ref neighbors);
                if (neighbors != null && neighbors.Count > 0)
                {
                    x_nearest = BestNearWithin(neighbors);
                }
                else
                {
                    Vactive.Neighbor_SingleQuery(a_sample, ref x_nearest, ref dist2_sample);
                    if (x_nearest == null)
                    {
                        //TODO fix this nearest query due to its limitation
                        return false;
                    }
                }

                //Propagation
                double iDt = double.MaxValue;
                system.SampleControl(rand, ref iCtrl, ref iDt);

                double[] q_new = x_new.Q;
                double iCost = double.MaxValue;
                bool propagate_success = system.Propagate(memoryPad, x_nearest.Q, iCtrl, iDt, ref q_new, ref iCost);
                if (propagate_success == false)
                    return false;

                double[] a_new = x_new.Alias;
                master.AliasOf(q_new, ref a_new);
                double cost_new = x_nearest.Cost + iCost;
                x_new.Cost = cost_new;
                e_new.step_cost = iCost;
                e_new.Dt = iDt;
                e_new.entry = x_nearest;
                e_new.exit = x_new;     // this is fine, e_new.exit will not be used by the master. The master will link the copied e_new.exit to a new x_new
                // remember x_new will be cloned in the master thread, so this link should point to the cloned state. So do the index field and parentEdge.
                    //x_new.parentEdge
                    //x_new.index
                    //e_new.exit

                if (system.ValidateEdge(e_new) == false)
                    return false;




                //Representation
                S.Neighbor_RangeQuery(a_new, ds, ref neighbors);
                s_new = null;

                int count = neighbors.Count;
                if (count > 0)
                {
                    int idx_r = rand.Next(count);

                    for (int i = idx_r; i < count; ++i)
                    {
                        Representation s_i = (Representation)neighbors[i];
                        if (cost_new <= s_i.X.Cost)
                            s_new = s_i;
                    }
                    for (int i = 0; i < idx_r; ++i)
                    {
                        Representation s_i = (Representation)neighbors[i];
                        if (cost_new <= s_i.X.Cost)
                            s_new = s_i;
                    }

                    if (s_new == null)
                        return false;
                }

                return true;
            }

            protected State BestNearWithin(List<State> neighbors)
            {
                State x_best = null;
                double cost_min = double.MaxValue;
                foreach (State x_i in neighbors)
                {
                    if (cost_min >= x_i.Cost)
                    {
                        cost_min = x_i.Cost;
                        x_best = x_i;
                    }
                }
                return x_best;
            }

        }

        #region planning components

        protected State x_root;
        protected PointSet Vactive;
        protected PointSet S;
        protected BaseSystem system;
        protected double dv;
        protected double ds;
        protected int n_exec;
        protected int n_current;

        protected GoalCheck inXgoal;
        protected double cost_solution;
        protected State x_goal;

        #endregion

        #region operations

        protected Thread masterThread;
        protected List<Agent> agentPool;
        
        #endregion

        #region information

        Thread monitorThread;
        int _measurement_iteration_prev;
        DateTime _measurement_iteration_dateTime_prev;
        double _measurement_ips;
        double _measurement_average_cost;
        double _measurement_bin_values_mean;
        double _measurement_bin_values_std;
        float _measurement_bin_fill_rate;

        Thread vertexTranslationThread;
        Skin_G skin_vertices;
        List<State> skin_v1;
        List<State> skin_v2;

        #endregion

        public SST(BaseSystem system, double[] q_root, string initializations, GoalCheck goalChecker, GraphicsDevice device, Skin_G visualizer)
        {
            n_current = 0;
            string[] separators = null;
            string[] entries = null;

            separators = new string[] { "<R_v>", "</R_v>" };
            entries = initializations.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) throw new ArgumentException();
            this.dv = double.Parse(entries[1]);

            separators = new string[] { "<R_s>", "</R_s>" };
            entries = initializations.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) throw new ArgumentException();
            this.ds = double.Parse(entries[1]);

            separators = new string[] { "<BinQuery_Branches>", "</BinQuery_Branches>" };
            entries = initializations.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) throw new ArgumentException();
            int[] branches = TextAssistant.StringToIntArray(entries[1]);

            Vactive = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);
            S = new BinQuery(system.Min_Alias, system.Max_Alias, system.DOF_Alias, branches);

            cost_solution = double.MaxValue;
            x_goal = null;
            inXgoal = goalChecker;

            this.system = system;
            SetRoot(q_root);

            masterThread = new Thread(this.Execute);
            masterThread.Priority = ThreadPriority.Highest;

            vertexTranslationThread = new Thread(VisualizeVertices);
            vertexTranslationThread.Priority = ThreadPriority.Lowest;

            skin_v1 = new List<State>(10240);
            skin_v2 = new List<State>(10240);
            skin_vertices = visualizer;
            visualizer.UpdateDataContent(skin_v1);

            monitorThread = new Thread(Monitoring);
            monitorThread.Priority = ThreadPriority.Lowest;
        }

        public int AgentNumber
        {
            get { return agentPool.Count; }
            set
            {
                if (agentPool == null)
                {
                    agentPool = new List<Agent>(value); 
                    while (agentPool.Count < value)
                        agentPool.Add(null);
                }
                else
                {
                    throw new NotImplementedException();
                }
            }
        }

        public void Terminate()
        {
            foreach (Agent a in agentPool)
            {
                a.workingThread.Abort();
            }
            masterThread.Abort();
            vertexTranslationThread.Abort();
            monitorThread.Abort();
        }

        private void copyState(State x_src, ref State x_dst)
        {
            double[] q_dst = null;
            double[] a_dst = null;

            if (x_dst == null)
            {
                q_dst = new double[system.DOF_State];
                a_dst = new double[system.DOF_Alias];
                x_dst = new State(q_dst, a_dst);
            }
            else
            {
                q_dst = x_dst.Q;
                a_dst = x_dst.Alias;
            }

            x_dst.Cost = x_src.Cost;
            MathAssistant.vector_set(x_src.Q, ref q_dst);
            MathAssistant.vector_set(x_src.Alias, ref a_dst);
        }

        private void copyEdge(Edge e_src, ref Edge e_dst)
        {
            if (e_dst == null)
            {
                e_dst = new Edge();
                e_dst.control = new double[e_src.control.Length];
            }
            
            e_dst.entry = e_src.entry;
            e_dst.exit = e_src.exit;
            e_dst.step_cost = e_src.step_cost;
            e_dst.Dt = e_src.Dt;
            double[] u_dst = e_dst.control;
            MathAssistant.vector_set(e_src.control, ref u_dst);
        }

        public void Execute(object input)
        {
            n_exec = (int)input;
            int i_idx = 0;
            Representation s_tmp = null;
            State x_tmp = null;
            Edge e_tmp = null;

            while (n_current < n_exec)
            {
                Agent agent_i = agentPool[i_idx];
                if (agent_i == null)
                {
                    //initialize this agent
                    agentPool[i_idx] = new Agent(this, Vactive, S, dv, ds);
                }
                else if (agent_i.done == true)
                {
                    //read the result
                    copyState(agent_i.x_new, ref x_tmp);
                    copyEdge(agent_i.e_new, ref e_tmp);
                    s_tmp = agent_i.s_new;
                    //order a new task
                    agent_i.done = false;
                    //process the result
                    processIncoming(x_tmp, e_tmp, s_tmp);
                    n_current++;
                }
                //else, do nothing

                // circular sweep
                i_idx++;
                if (i_idx >= agentPool.Count)
                    i_idx = 0;
            }


        }

        public void SetRoot(double[] q_root)
        {
            x_root = SpawnState(q_root);
            Vactive.AddState(x_root);

            Representation s_root = new Representation(x_root.Alias, x_root);
            S.AddState(s_root);
        }

        public void Start(int iterations)
        {
            masterThread.Start(iterations);
            monitorThread.Start();
        }

        protected State SpawnState(double[] q)
        {
            if (q == null)
                throw new ArgumentException();

            State x = new State(q, AliasOf(q));

            return x;
        }

        private void processIncoming(State x, Edge e, Representation s)
        {
            //if (x.Cost > cost_solution)
            //    return;

            State x_new = GrowGFrom(x, e);
            
            if (inXgoal(x_new.Q))
            {
                if (cost_solution >= x_new.Cost)
                {
                    cost_solution = x_new.Cost;
                    x_goal = x_new;
                }
            }
            
            if (s == null)
            {
                S.AddState(new Representation(x_new.Alias, x_new));
            }
            else
            {
                State x_peer = s.X; // x_peer must be worse than x_new, otherwise the agent will not hand it in
                s.X = x_new;
                Vactive.DelState(x_peer);
            }
        }

        private State GrowGFrom(State x, Edge e)
        {
            double[] e_control = new double[system.DOF_Control];
            MathAssistant.vector_set(e.control, ref e_control);
            Edge e_new = new Edge();
            e_new.control = e_control;
            e_new.Dt = e.Dt;
            e_new.entry = e.entry;
            e_new.step_cost = e.step_cost;

            double[] q_new = new double[system.DOF_State];
            double[] a_new = new double[system.DOF_Alias];
            MathAssistant.vector_set(x.Q, ref q_new);
            MathAssistant.vector_set(x.Alias, ref a_new);
            State x_new = new State(q_new, a_new);
            x_new.Cost = x.Cost;
            x_new.ParentEdge = e_new;

            e_new.exit = x_new;

            Vactive.AddState(x_new);

            return x_new;
        }

        public void VisualizeVertices()
        {
            while (true)
            {
                List<State> backList = skin_v1;
                if (skin_vertices.isBeingRendered(backList))
                    backList = skin_v2;

                double bin_value_mean = 0;
                double bin_value_variance = 0;
                int bin_value_non_empty = 0;
                ((BinQuery)Vactive).ReportVerticesSnapshot(ref backList, ref bin_value_mean, ref bin_value_variance, ref bin_value_non_empty);
                skin_vertices.UpdateDataContent(backList);

                double averageCost_new = 0;
                int n_x = backList.Count;
                int n_x_null = 0;
                foreach (State x in backList)
                {
                    if (x != null)
                        averageCost_new += x.Cost;
                    else
                        n_x_null++;
                }
                averageCost_new = averageCost_new / (n_x - n_x_null);
                _measurement_average_cost = Math.Floor(averageCost_new * 100000)/100000;
                _measurement_bin_values_mean = bin_value_mean;
                _measurement_bin_values_std = Math.Sqrt(bin_value_variance);
                _measurement_bin_fill_rate = (float)bin_value_non_empty / ((BinQuery)Vactive).NumberValueBins;

                Thread.Sleep(100);
            }
        }

        public void Monitoring()
        {
            DateTime currentDT = DateTime.Now;
            double time_interval_in_seconds = 1;
            int delta_iterations = 0;
            while (true)
            {
                currentDT = DateTime.Now;

                time_interval_in_seconds = (currentDT - _measurement_iteration_dateTime_prev).TotalSeconds;
                _measurement_iteration_dateTime_prev = currentDT;

                if (masterThread.ThreadState == System.Threading.ThreadState.Running)
                {
                    int n_current_new = 0;
                    for (int i = 0; i < agentPool.Count; ++i)
                    {
                        Agent a_i = agentPool[i];
                        if(a_i!=null)
                            n_current_new += agentPool[i]._measurement_n_tasks;
                    }
                    n_current = n_current_new;

                    delta_iterations = n_current - _measurement_iteration_prev;
                    _measurement_iteration_prev = n_current;

                    _measurement_ips = Math.Floor((delta_iterations / time_interval_in_seconds) * 10) / 10;
                }

                Thread.Sleep(100);
            }
        }

        public void GetStatistics(ref List<string> info)
        {
            if (info == null)
                info = new List<string>(1);

            while (info.Count < 8)
                info.Add(null);

            info[0] = "ips = " + _measurement_ips;
            info[1] = "n_current = " + n_current;
            info[2] = "Vactive.count = " + Vactive.Count;
            info[3] = "average cost = " + _measurement_average_cost;
            info[4] = "bin.count.mean = " + _measurement_bin_values_mean;
            info[5] = "bin.count.std = " + _measurement_bin_values_std;
            info[6] = "bin.cellFillRate = " + _measurement_bin_fill_rate;

            if(cost_solution > double.MaxValue/2)
                info[7] = "cost_best = n/a";
            else
                info[7] = "cost_best = " + cost_solution;
        }

        public Skin_G Skin
        {
            get { return skin_vertices; }
        }

        public bool Viewable
        {
            get
            {
                if (vertexTranslationThread.ThreadState == System.Threading.ThreadState.Running)
                    return true;
                else
                    return false;
            }
            set
            {
                if (value == true)
                {
                    vertexTranslationThread.Start();
                }
                else
                {
                    if (vertexTranslationThread.ThreadState == System.Threading.ThreadState.Running)
                        vertexTranslationThread.Join();
                }
            }
        }

        public State x_solution
        {
            get { return x_goal; }
        }

        public double[] AliasOf(double[] q)
        {
            double[] alias = new double[system.DOF_Alias];
            AliasOf(q, ref alias);
            return alias;
        }

        public void AliasOf(double[] q, ref double[] alias)
        {
            if (alias == null)
                alias = new double[system.DOF_Alias];
            for (int i = 0; i < system.DOF_Alias; ++i)
                alias[i] = q[i];
        }

    }
}
